import cv2,glob
import time
import logging
import numpy as np
from typing import Tuple, Optional
import ColorDetect as cd
from multiprocessing import Process, Manager

# 配置日志
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')
logger = logging.getLogger(__name__)

def find_available_camera():
    """查找可用的摄像头设备"""
    video_devices = glob.glob('/dev/video*')
    for device in sorted(video_devices):
        device_num = int(device.split('video')[1])
        cap = cv2.VideoCapture(device_num)
        if cap.isOpened():
            cap.release()
            return device_num
    return None

class Camera:
    """摄像头控制类，提供视频捕获功能"""
    
    def __init__(self, camera_id: int = 0, frame_width: int = 640, frame_height: int = 480, fps: int = 30):
        """
        初始化摄像头
        Args:
            camera_id: 摄像头设备ID，默认为0
            frame_width: 帧宽度，默认640
            frame_height: 帧高度，默认480
            fps: 帧率，默认30
        """
        self.camera_id = camera_id
        self.frame_width = frame_width
        self.frame_height = frame_height
        self.fps = fps
        self.cam = None
        self.is_initialized = False
        self.cap_process = None
        self.center_process = None
        self.color_process = None
        self.running = False
        self.manager = Manager()
        self.shared_dict = self.manager.dict()
        self.shared_dict['cam'] = False
        self._init_mp()
        self._init_camera()
        
        self.is_ok=True
        
    
    def _init_camera(self):
        self.shared_dict['cam'] = False
        while self.shared_dict['cam'] == False:
            if self.cam is None:
                logger.warning("开启摄像头")
                cam_index=find_available_camera()
                if cam_index is not None:
                    self.camera_id = cam_index
            else:
                logger.warning("重启摄像头")
                self.cam.release()
                cam_index=find_available_camera()
                
                if cam_index is not None:
                    self.camera_id = cam_index
            """初始化摄像头"""
            try:
                self.cam = cv2.VideoCapture(self.camera_id, cv2.CAP_V4L2)
                if not self.cam.isOpened():
                    raise RuntimeError("无法打开摄像头")
                
                # 设置摄像头参数
                self.cam.set(cv2.CAP_PROP_FRAME_WIDTH, self.frame_width)
                self.cam.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frame_height)
                self.cam.set(cv2.CAP_PROP_FPS, self.fps)
                
                # 预热摄像头
                for _ in range(5):
                    ret, _ = self.cam.read()
                    if not ret:
                        raise RuntimeError("摄像头预热失败")
                    time.sleep(0.05)
                logger.info("摄像头初始化成功")
                self.shared_dict['cam'] = True
            except Exception as e:
                logger.error(f"摄像头初始化失败: {e}")
                self.cam.release()
                self.cam = None
                time.sleep(1)
                raise
    
    def _init_mp(self):
        """初始化多进程资源"""
        
        self.shared_dict['area'] = 400
        self.shared_dict['frame'] = None
        self.shared_dict['centers'] = None
        self.shared_dict['color'] = None
        self.shared_dict['cap_running'] = False
        self.shared_dict['center_running'] = False
        self.shared_dict['color_running'] = False
        self.shared_dict['possesed_pics'] = False
        self.shared_dict['line_running'] = False
        self.shared_dict['line_angle']= None
        self.shared_dict['can_get_frame']= False
        self.is_initialized = True
        
        logger.info("摄像头系统初始化成功")

    def start(self):
        """启动摄像头捕获进程"""
        if self.shared_dict['cap_running']==True:
            logger.warning("摄像头捕获已经在运行")
        else:
            self.shared_dict['cap_running'] = True
            self.cap_process = Process(target=self._capture_process)
            self.cap_process.daemon = True  # 设置为守护进程，主进程结束时自动终止
            self.cap_process.start()
            logger.info("摄像头捕获进程已启动")
        if self.shared_dict['center_running']==True:
            logger.warning("中心检测进程已经在运行")
        elif self.shared_dict['cap_running']==False:
            logger.warning("请先启动摄像头捕获进程")
        else:
            self.shared_dict['center_running'] = True
            self.center_process = Process(target=self.get_center_p)
            self.center_process.daemon = True  # 设置为守护进程，主进程结束时自动终止
            self.center_process.start()
            logger.info("中心检测进程已启动")
        if self.shared_dict['color_running']==True:
            logger.warning("颜色捕获进程已经在运行")
        elif self.shared_dict['cap_running']==False:
            logger.warning("请先启动摄像头捕获进程")
        else:
            self.shared_dict['color_running'] = True
            self.color_process = Process(target=self.get_color_p)
            self.color_process.daemon = True  # 设置为守护进程，主进程结束时自动终止
            self.color_process.start()
            logger.info("颜色捕获进程已启动")

        if self.shared_dict['line_running']==True:
            logger.warning("直线检测进程已经在运行")
        elif self.shared_dict['cap_running']==False:
            logger.warning("请先启动摄像头捕获进程")
        else:
            self.shared_dict['line_running'] = True
            self.line_process = Process(target=self.get_longest_line_angle_p)
            self.line_process.daemon = True  # 设置为守护进程，主进程结束时自动终止
            self.line_process.start()
            logger.info("直线检测进程已启动")
        self.is_ok=True

    def get_is_ok():
        """获取摄像头是否正常工作"""
        return self.is_ok
    def stop(self):
        """停止摄像头捕获进程"""
        if not self.shared_dict.get('cap_running', False):
            pass
        else:
            self.shared_dict['cap_running'] = False          
            if self.cap_process and self.cap_process.is_alive():
                # 给进程一点时间自行退出
                time.sleep(0.2)
                # 如果进程仍在运行，强制终止
                if self.cap_process.is_alive():
                    self.cap_process.terminate()
                    self.cap_process.join(timeout=1.0)
                
                logger.info("摄像头捕获进程已停止")
                self.cap_process = None
        if not self.shared_dict.get('center_running', False):
            pass
        else:
            self.shared_dict['center_running'] = False          
            if self.center_process and self.center_process.is_alive():
                # 给进程一点时间自行退出
                time.sleep(0.2)
                # 如果进程仍在运行，强制终止
                if self.center_process.is_alive():
                    self.center_process.terminate()
                    self.center_process.join(timeout=1.0)
                
                logger.info("中心检测进程已停止")
                self.center_process = None
        if not self.shared_dict.get('color_running', False):
            pass
        else:
            self.shared_dict['color_running'] = False          
            if self.color_process and self.color_process.is_alive():
                # 给进程一点时间自行退出
                time.sleep(0.2)
                # 如果进程仍在运行，强制终止
                if self.color_process.is_alive():
                    self.color_process.terminate()
                    self.color_process.join(timeout=1.0)
                logger.info("颜色捕获进程已停止")
                self.color_process = None

    def _capture_process(self):
        """多进程模式下的帧捕获函数"""
        # 在子进程中创建自己的摄像头连接
        cam = self.cam
        flg=0
        try:
            while self.shared_dict['cap_running']:
                if self.shared_dict['cam'] == False:
                    continue
                ret, frame = cam.read()
                if ret:
                    self.shared_dict['can_get_frame']= True
                    self.shared_dict['frame'] = frame
                else:
                    self.shared_dict['can_get_frame']= False
                    flg+=1
                    logger.warning(f"读取摄像头帧失败{flg}次，尝试重新连接摄像头")
                    self.is_ok=False
                    if flg>5:
                        logger.warning(f"重新连接摄像头1")
                        self.shared_dict['cam'] = False
                        while self.shared_dict['cam'] == False:
                            try:
                                self._init_camera()
                                cam = self.cam
                            except Exception as e:
                                print(e)
                        logger.warning(f"重新连接摄像头2")
                        time.sleep(2)
                        flg=0
                        
                    time.sleep(1.0 / self.fps)
                # 控制帧率，避免CPU占用过高
                time.sleep(1.0 / self.fps)
        except Exception as e:
            logger.error(f"摄像头捕获进程异常: {e}")
            self.is_ok=False
            self.start()
        finally:
            cam.release()
            logger.info("摄像头捕获进程结束")

    def get_frame(self) -> Tuple[bool, Optional[np.ndarray]]:
        """
        获取摄像头帧
        
        Returns:
            Tuple[bool, Optional[np.ndarray]]: (是否成功, 图像数据)
        """
        # 如果进程在运行，从共享内存获取帧
        if self.shared_dict.get('cap_running', False):
            frame = self.shared_dict.get('frame')
            return frame is not None, frame
        
        # 如果进程未运行，直接从摄像头读取
        if not self.cam.isOpened():
            logger.info("摄像头未打开")
            return False, None
        if self.cam is None:
            logger.info("无摄像头")
            self.is_ok=False
            return False, None
        logger.info("进程未打开，直接从摄像头读取")
        self.shared_dict['cam'] = False
        return self.cam.read()
    
    def get_center_p(self):
        """
        获取各个颜色物体的中心坐标
        Returns:
            物体中心坐标
        """
        while self.shared_dict['center_running']:
            if self.shared_dict['can_get_frame'] == False:
                    self.shared_dict['centers'] = None
                    continue
            ret, frame = self.get_frame()
            if not ret or frame is None:
                logger.warning("无法获取帧进行中心检测")
                self.shared_dict['centers'] = None
            try:
                centers=[]
                possesed_pics=[]
                for i in range(3):
                    res=cd.get_center(frame, i+1)
                    centers.append(res[0])
                    possesed_pics.append(res[1])
                self.shared_dict['centers'] = centers
                self.shared_dict['possesed_pics']=possesed_pics
            except Exception as e:
                logger.error(f"中心检测失败: {e}")
                self.shared_dict['centers'] = None
            time.sleep(1.0 / self.fps)

    def get_center(self, color):
        try:
            center=None
            centers=self.shared_dict['centers']
            if centers:
                center=centers[color-1]
                
            return center
        except Exception as e:
            logger.error(f"中心获取失败: {e}")


    def get_possesed_pic(self):
        
        
        try:
            possesed_pics=self.shared_dict['possesed_pics']
            if possesed_pics[0]==None:
                return None
            else:
                cv2.imwrite(f'/root/why_agv/pic/red.jpg', possesed_pics[0])
                cv2.imwrite(f'/root/why_agv/pic/green.jpg', possesed_pics[1])
                cv2.imwrite(f'/root/why_agv/pic/blue.jpg', possesed_pics[2])
        except Exception as e:
            logger.error(f"中心结果获取失败: {e}")

    def get_color_p(self):
        """
        获取指定颜色物体的中心坐标

        Args:
            img: 图像数据
            area: 色块大小

        Returns:
            物体中心坐标
        """
        while self.shared_dict['color_running']:
            if self.shared_dict['can_get_frame'] == False:
                    self.shared_dict['color'] = None
                    continue
            ret, frame = self.get_frame()
            if not ret or frame is None:
                logger.warning("无法获取帧进行颜色检测")
                self.shared_dict['color'] = None
                
            try:
                area = self.shared_dict['area']
                self.shared_dict['color'] = cd.get_color(frame, area)
            except Exception as e:
                logger.error(f"颜色检测失败: {e}")
                self.shared_dict['color'] = None
            time.sleep(1.0 / self.fps)

    def get_color(self,area):
        try:
            self.shared_dict['area']=area
            return self.shared_dict['color']
        except Exception as e:
            logger.error(f"颜色获取失败: {e}")
    
    def get_longest_line_angle_p(self):
        """
        获取图像中最长线的角度
        """
        while self.shared_dict['line_running']:
            if self.shared_dict['can_get_frame'] == False:
                    self.shared_dict['line_angle'] = None
                    continue
            ret, frame = self.get_frame()
            if not ret or frame is None:
                logger.warning("无法获取帧进行直线检测")
                self.shared_dict['line_angle'] = None

            try:
                self.shared_dict['line_angle'] = cd.get_longest_line_angle(frame)
            except Exception as e:
                logger.error(f"直线检测失败: {e}")
                self.shared_dict['line_angle'] = None
            time.sleep(1.0 / self.fps)

    def get_longest_line_angle(self):   
        try:
            return self.shared_dict['line_angle']
        except Exception as e:
            logger.error(f"直线角度获取失败: {e}")





    def is_opened(self) -> bool:
        """
        检查摄像头是否打开
        
        Returns:
            bool: 摄像头是否打开
        """
        return self.cam is not None and self.cam.isOpened()
    
    def take_pic(self,name):
        """
        拍照并保存到指定路径
        """
        # possesed_pics=self.shared_dict['possesed_pics']
        # possesed_pic=possesed_pics[2]
        img=self.get_frame()[1]
        
        cv2.imwrite(f'/root/why_agv/pic/{name}.jpg', img)
        return 1


    def release(self):
        """释放摄像头资源"""
        try:
            self.stop()  # 先停止捕获进程
            
            if self.cam is not None:
                self.cam.release()
                self.cam = None
                self.is_initialized = False
                logger.info("摄像头资源已释放")
        except Exception as e:
            logger.error(f"释放摄像头资源失败: {e}")
    
    def __enter__(self):
        """上下文管理器入口"""
        return self
    
    def __exit__(self, exc_type, exc_val, exc_tb):
        """上下文管理器出口"""
        self.release()
    
    def __del__(self):
        """析构函数"""
        self.release()


# 使用示例
if __name__ == "__main__":
    with Camera(camera_id=0) as cam1:
        print("初始化成功！")
        
        # 启动后台捕获进程
        cam1.start()

        while 1:
            cam1.get_possesed_pic()
            cen=cam1.get_center(3)
            if cen:
                print(cen[1]-320,cen[0]-240)
            else:
                print(cen)
                time.sleep(2)
            time.sleep(0.1)
    # try:
    #     # 使用上下文管理器确保资源正确释放
    #     with Camera(camera_id=0) as cam:
    #         print("初始化成功！")
            
    #         # 启动后台捕获进程
    #         cam.start()

    #         while 1:
    #             cam.get_possesed_pic()
    #             cen=cam.get_center(3)
    #             print(cen)
    #             time.sleep(1.0)
               
    # except KeyboardInterrupt:
    #     print("程序被用户中断")
    # except Exception as e:
    #     print(f"程序运行错误: {e}")
    # finally:
    #     print("程序结束")
